#!/usr/bin/env python

import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError
import time

def call_back(data):
	try:
		bridge = CvBridge()
		cv_image = bridge.imgmsg_to_cv2(data, 'bgr8')

		cv2.imshow('src', cv_image)
		cv2.waitKey(1)

		template = cv2.imread('/home/daisy/catkin_test/src/my_pkg/src/template_coke.png')

		#cv2.imshow('template', template)

		#cv2.waitKey(1)

		h, w = template.shape[:2]


		res = cv2.matchTemplate(cv_image, template, cv2.TM_SQDIFF)


		min_val, max_val, min_loc, max_loc = cv2.minMaxLoc(res)


		top_left = min_loc

		bottom_right = (top_left[0] + w, top_left[1] + h)
		
		

		cv2.rectangle(cv_image, top_left, bottom_right, (0,0,255), 2)

		cv2.imshow('dst',cv_image)

		cv2.waitKey(1)

		
	except e:
		print(e)

if __name__ == '__main__':
	
	#户
	rospy.init_node('take_photo', anonymous = False)
	
	img_topic = '/camera1/color/image_raw'
	image_sub = rospy.Subscriber(img_topic, Image, call_back)

	rospy.sleep(1)
	
	while 1:
		pass

